// Copyright 2006-2012 Brendan Burns
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
// "Software"), to deal in the Software without restriction, including
// without limitation the rights to use, copy, modify, merge, publish,
// distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to
// the following conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#include <termios.h>
#include <unistd.h>
#include <stdio.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdlib.h>
#include "nmc_low_level.h"


int main(int argc, char **argv) {
  int ret;
  int fd;
  int i;
  char msg[20];
  int nummod;
  int type, version;
  long counter  = 0;
  struct timeval start, now;
  
  fd = nmc_connect(argv[1], 115200, &nummod);
  for (i=1;i<=nummod;i++) {
    nmc_init_module(fd, i);
    printf("%d: %d %d\n", i, modules[i-1].type, modules[i-1].version);
  }

  
  for (i=1;i<=nummod;i++) {
    nmc_define_status(fd, i, SEND_POS);
  }
  
  for (i=1;i<=nummod;i++) {
    nmc_motor_stop(fd, i, AMP_ENABLE | MOTOR_OFF);
    nmc_motor_stop(fd, i, AMP_ENABLE | STOP_ABRUPT);
    nmc_reset(fd, i);
  }
  
  nmc_set_group_internal(fd, 1, 0x81, 1);
  nmc_set_group_internal(fd, 2, 0x81, 0);
  nmc_set_group_internal(fd, 3, 0x81, 0);
  nmc_set_group_internal(fd, 4, 0x81, 0);
  nmc_set_group_internal(fd, 5, 0x81, 0);


 /*
   gettimeofday(&start, NULL);
   while (1) {
   for ( i=1;i<=nummod;i++) {
   nmc_get_status(fd, i);
   }
   if ((++counter % 100) == 0) {
   gettimeofday(&now, NULL);
   double elapsed = now.tv_sec-start.tv_sec+
   ((float)(now.tv_usec-start.tv_usec))/1000000;
   printf("Time: %g  Average: %g  Hz: %g %ld\n", 
   elapsed, (elapsed/counter), 1/(elapsed/counter), counter);
   }
   }
 */
 
 close(fd);
}
    
